#include "algorithm.h"
#include <math.h>

pid_control_t hh = {0};
static const char *TAG = "Algorithm";

void pid_init(pid_control_t *pid, float Kp, float Ki, float Kd, float max_output)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->max_output = max_output;
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->prev_derivative = 0.0f;
    pid->tau = 0.02f; // 默认滤波时间常数20ms
}

float pid_update(pid_control_t *pid, float error, float dt)
{
    // 检查输入有效性
    if (isnan(error) || isinf(error) || dt <= 0.0f)
    {
        return 0.0f;
    }

    // === 比例项 ===
    float P = pid->Kp * error;

    // === 积分项（梯形积分）===
    float integral_increment = 0.5f * (error + pid->prev_error) * dt;
    pid->integral += integral_increment;
    float I = pid->Ki * pid->integral;

    // === 微分项（带低通滤波）===
    float derivative = 0.0f;
    if (dt > 1e-6f)
    { // 避免除以零
        derivative = (error - pid->prev_error) / dt;
    }

    // 应用一阶低通滤波
    float alpha = dt / (pid->tau + dt);
    float filtered_derivative = (1.0f - alpha) * pid->prev_derivative + alpha * derivative;
    float D = pid->Kd * filtered_derivative;

    // === PID 输出 ===
    float output = P + I + D;

    // === 输出限幅和抗积分饱和 ===
    if (fabsf(output) > pid->max_output)
    {
        output = (output > 0) ? pid->max_output : -pid->max_output;
        // 抗饱和：回退当前步长的积分增量
        pid->integral -= integral_increment;
    }

    // 更新状态
    pid->prev_derivative = filtered_derivative;
    pid->prev_error = error;

    return output;
}